Reduce MPPI computation time, add OpenMP support, PathAlignCritic: Improve occupancy check, Add DirectionChangeCritic#11
Reduce MPPI computation time, add OpenMP support, PathAlignCritic: Improve occupancy check, Add DirectionChangeCritic#11adivardi wants to merge 32 commits intoenway-develfrom
Conversation
Summary of ChangesHello, I'm Gemini Code Assist1! I'm currently reviewing this pull request and will post my feedback shortly. In the meantime, here's a summary to help you and other reviewers quickly get up to speed! This pull request introduces a comprehensive overhaul of the MPPI controller's visualization framework, providing users with much finer control over debugging and analysis outputs. Alongside these visualization enhancements, a new critic has been integrated to promote smoother robot motion by penalizing sudden direction changes. Significant performance gains are also achieved through the introduction of OpenMP parallelization in the core cost evaluation, making the controller more efficient and robust. Highlights
Using Gemini Code AssistThe full guide for Gemini Code Assist can be found on our documentation page, here are some quick tips. Invoking Gemini You can request assistance from Gemini at any point by creating a comment using either
Customization To customize Gemini Code Assist for GitHub experience, repository maintainers can create a configuration file and/or provide a custom code review style guide (such as PEP-8 for Python) by creating and adding files to a Limitations & Feedback Gemini Code Assist may make mistakes. Please leave feedback on any instances where its feedback is incorrect or counter productive. You can react with 👍 and 👎 on @gemini-code-assist comments. If you're interested in giving your feedback about your experience with Gemini Code Assist for GitHub and other Google products, sign up here. Footnotes
|
![gemini-code-assist[bot]](https://avatars.githubusercontent.com/in/956858?s=60&v=4){kind=small}
There was a problem hiding this comment.
Code Review
This pull request significantly enhances the nav2_mppi_controller by refactoring and expanding its visualization capabilities, introducing a new DirectionChangeCritic, and implementing performance optimizations. The TrajectoryVisualizer now supports publishing various data including candidate trajectories with total and individual critic costs, optimal trajectories, paths, and footprints, controlled by new parameters. OpenMP parallelization has been integrated into the CostCritic for improved performance, along with optimizations to collision checking and Eigen matrix operations. A new DirectionChangeCritic is added to penalize changes in driving direction. A review comment highlights an unimplemented feature related to publish_transformed_path that is being tested, suggesting either implementation or test removal. Another comment points out an unused variable in the DirectionChangeCritic implementation that should be removed for code cleanliness.
adivardi
force-pushed
the
av/mppi_freq
branch
2 times, most recently
from
17a55cd to
5a96778
Compare
adivardi
changed the title
Use robot_speed from feedback, as it better represents the actual direction of motion. This results in more consistent critic application and longer motions in each direction
Not sure if useful
After looking into adding break early when computing the path's arc-length
The clamp (min(80 * temperature)) caps all exp() arguments at -80, where the value is already ~1e-35, slightly above float32's smallest fraction of ~1.2e-38. This eliminates potential underflows &= slow paths in the exp implementation. This reduces computation by ~6x, from 2000-300us to ~400us
Let's Eigen know there is no aliasing, very small optimization
Reduces computation time from 18-20ms to 5ms
This reverts commit 9d721b9.
georgflick
left a comment
georgflick
left a comment
There was a problem hiding this comment.
don't you need to add the openMP dependency also to the package.xml ? or are you not adding it, because it's optional?
| @@ -0,0 +1,51 @@ | |||
| // Copyright (c) 2024 Enway GmbH, Adi Vardi | |||
| @@ -0,0 +1,69 @@ | |||
| // Copyright (c) 2024 Enway GmbH, Adi Vardi | |||
yeah. I copied from ros-navigation#5933 What do you think? I guess I will also PR this upstream at some point |
adivardi
changed the title
2 participants
Additions & Changes
PathAlign Critic
Add a minimum distance for the occupancy check.
Previously, the occupancy check was done up to a distance = furthest_reached_path_point along the path,
Now, it is done over a
distance = min(min_dist, furthest_reached_path_point).The effect of this change is that when approaching an obstacle, even if the robot slows down (and thus
furthest_reached_path_pointdistance becomes smaller and smaller), the check would still trigger and disable the critic. This solves the issue where the robot slows down when approaching an obstacle instead of overtaking it, until it gets stuck when the obstacle is just in front of the robot.Results of this change are susbtiantal:
Previously, robot was not able to overtake a box of 1.5m width places in front of it.
With the change, the robot can overtake obstacles up to the width of a car and even more.
Remaining issues
When approaching a car parked perpendicular to the robot, the robot correctly starts going around it. However, when the robot is perpendicular to the path, MPPI finds no good solution and "gets lost"
DirectionChange Critic
Add a critic for penalizing changes in driving direction. This reduces MPPI switching direction too quickly, leading to robot to get stuck as the motors cannot follow.
Applying the critic on the current robot's speed (and not last command published) leads to much better results
Added a new
state.robot_speedfield asstate.speedis either current speed or last command published, depends on open_loop setting.Remaining issues
DirectionChange: Also penalize wz to avoid quick steering in place when stuckMPPI Frequency
This PR improves the runtime frequency of the MPPI controller. The goal is to reduce the occurrences of the controller's frequency dropping below the requested 20Hz, which causes degraded performance.
The main culprit in MPPI are the collision checks, which take a big percentage of the runtime, especially when driving near a long obstacles (such as a wall)
Changes:
Cost critic:
costmap_ros_->getRobotFootprint())onceOptimizer:
noaliascallChecklist (tick off when done or remove when not relevant):